#include <iostream>
#include <Eigen/Geometry>
#include <sophus/se3.hpp>

using namespace std;
using namespace Eigen;
using namespace Sophus;

typedef Eigen::Matrix<double, 6, 1> Vector6d;

int main()
{
    AngleAxisd r(M_PI_2, Vector3d::UnitY());

    Quaterniond q(r);

    Vector3d t(1, 0, 0);

    SE3d se3_rt(r.matrix(), t);
    SE3d se3_qt(q, t);

    cout << "SE3 from r,t: \n"
         << se3_rt.matrix() << endl;
    cout << "SE3 from q,t: \n"
         << se3_qt.matrix() << endl;

    Vector6d se3 = se3_rt.log();
    cout << "se3 = " << se3.transpose() << endl;
    cout << "se3 hat = " << SE3d::hat(se3) << endl;
    cout << "se3 vee = " << SE3d::vee(SE3d::hat(se3)).transpose() << endl;

    Vector6d se3_delta;
    se3_delta.setZero();
    se3_delta(0, 0) = 1e-4;

    SE3d se3_update = SE3d::exp(se3_delta) * se3_rt;
    cout << "SE3 update: \n"
         << se3_update.matrix() << endl;

    return 0;
}